#ifndef FREE_FALLING_SYSTEM_HPP
#define FREE_FALLING_SYSTEM_HPP
#include <math.h>
#include "EKF.hpp"

/**
 * @brief Definition of system to be estimated by ukf
 * @note state = [px,vx,py,vy]
 * @note 4
 */

class PoseSystem {
public:
    typedef float T;
    typedef Eigen::Matrix<T, 4, 1> Vector4t;
    typedef Eigen::Matrix<T, 4, 4> Matrix4t;

    typedef Eigen::Matrix<T, 2, 1> Vector2t;
    typedef Eigen::Matrix<T, 2, 4> Matrix24t;

    typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXt;
public:
    PoseSystem(double delta_t = 0.1)
        : dt(delta_t)
    {}

    //!< predict
    // system equation: from state_k to predict state_{k+1}
    // control:
    //      Vector4t f(const Vector4t& state, const VectorXt& control) const {
    Vector4t f(const Vector4t& state) const {
        Vector4t next_state;
        next_state(0) = state(0) + state(1)*dt;
        next_state(1) = state(1) - kx*state(1)*state(1)*dt;
        next_state(2) = state(2) + state(3)*dt;
        next_state(3) = state(3) + (ky*state(3)*state(3) - gravity)*dt;

        return next_state;
    }
    // Jacobian of process model
    Matrix4t F(const Vector4t& state) const {
        Matrix4t JacobianF = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(4,4);
        JacobianF(0,0) = JacobianF(2,2) = 1;
        JacobianF(0,1) = JacobianF(2,3) = dt;
        JacobianF(1,1) = 1-2*kx*state(1)*dt;
        JacobianF(3,3) = 1+2*ky*state(3)*dt;

        return JacobianF;
    }

    //!< update
    // observation equation
    Vector2t h(const Vector4t& state) const {
        Vector2t radar_measure;
        radar_measure(0) = std::sqrt(state(0)*state(0) + state(2)*state(2));
        radar_measure(1) = atan(state(0)/state(2));
        return radar_measure;
    }
    // Jacobian of measurement model
    Matrix24t H(const Vector4t& state) const {
        Matrix24t JacobianH = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(2,4);
        T rho = std::sqrt(state(0)*state(0) + state(2)*state(2));

        JacobianH(0,0) = state(0) / rho;
        JacobianH(0,2) = state(2) / rho;
        T temp = 1 + (state(0)/state(2))*(state(0)/state(2));
        JacobianH(1,0) = 1.f/(state(2) * temp);
        JacobianH(1,2) = -state(0) / (state(2)*state(2)*temp);
        return JacobianH;
    }


public:
    double dt;
    const double kx = 0.01, ky = 0.05;
    const double gravity = 9.8;
};


#endif
